/*
 * @Author: EltonLi lijiaming@uavi-tech.com
 * @Date: 2023-09-12 17:48:47
 * @LastEditors: LVGRAPE
 * @LastEditTime: 2025-05-20 16:38:39
 * @FilePath: \zino-fc-v4\applications\ZINO_FC_V4.0\statecontrol\statecontrol.h
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#ifndef __STATECONTROL_H__
#define __STATECONTROL_H__

#ifdef __cplusplus
extern "C"
{
#endif

#include <rtdef.h>
#include "fc_sensors.h"
#include "usercmd.h"
#define RATE_100_HZ 100
#define RATE_200_HZ 200
#define RATE_250_HZ 250
#define RATE_500_HZ 500
#define RATE_1000_HZ 1000
#define MAIN_LOOP_RATE RATE_1000_HZ
#define MAIN_LOOP_DT (u32)(1000 / MAIN_LOOP_RATE)

#define RATE_DO_EXECUTE(RATE_HZ, TICK) ((TICK % (MAIN_LOOP_RATE / RATE_HZ)) == 0)

    enum dir_e // maxi:前后左右，四个翻滚的方向。
    {
        CENTER = 0,
        FORWARD,
        BACK,
        LEFT,
        RIGHT,
    };

    typedef union statecontrol
    {
        struct
        {
            float roll;
            float pitch;
            float yaw;
            float thrust;
            float MixerRange;
            enum dir_e flipDir; /*翻滚方向*/
        };
        float data[6];
    } fc_control_t; // PID输出值
    extern fc_control_t fc_control;
    extern attitude_t attitudeExpect;
    extern attitude_t attitudeRateExpect;
    void statecontrol_init(void);
    void statecontrol(fc_control_t *out, setPoint_t *set, fc_sensor_data_t *sensor, zdrone_state_t *state, float dt);
    void position_control(fc_control_t *out, setPoint_t *set, fc_sensor_data_t *sensor, zdrone_state_t *state, float dt);
#ifdef __cplusplus
}
#endif

#endif
